
#define hall_a (1<<24)
#define hall_b (1<<25)
#define hall_c (1<<26)
#define ph_a_high (1<<16)
#define ph_a_low (1<<17)
#define ph_b_high (1<<18)
#define ph_b_low (1<<19)
#define ph_c_high (1<<20)
#define ph_c_low (1<<21)
#define power_on (1<<10)


#define P0DIR  FIO0DIR
#define P0SET  FIO0SET
#define P0CLR  FIO0CLR
#define P0PIN  FIO0PIN

#define P0PIN1 FIO0PIN1
#define P0PIN2 FIO0PIN2
#define P0PIN3 FIO0PIN3
#define P0PIN4 FIO0PIN4

#define P1DIR  FIO1DIR
#define P1SET  FIO1SET
#define P1CLR  FIO1CLR
#define P1PIN  FIO1PIN

#define P1PIN1 FIO1PIN1
#define P1PIN2 FIO1PIN2
#define P1PIN3 FIO1PIN3
#define P1PIN4 FIO1PIN4


#define TASK_STACK_SIZE             300
#define PID_TASK_PRIORITY           5
#define Hall_TASK_PRIORITY          4
#define LOGGING_TASK_PRIORITY       6
#define COUNTING_TASK_PRIORITY      7

void set_pwm(unsigned pwm_duty);
void pwm_init(void);
int precharge_delay=200;
unsigned int pwm_duty=2;

typedef struct
{
   uint32_t frequency;            // Blinking frequency
} sTaskParams_t, *spTaskParams_t;

sTaskParams_t HallArgs =    {
                            2000    // Task Execution Frequency
                            };


static void   HallTask            ( void *pvParameters );



extern "C" void bootup( void )
{
portENTER_CRITICAL();
{
    SCS=3;      //set GPIO

    // Setting Multiplier and Divider values
    PLLCFG=0x25; // M = 6
    PllFeed();
    
    // Enabling the PLL */
    PLLCON=0x1;
    PllFeed();
    
#define PLOCK 0x400
    // Wait for the PLL to lock to set frequency
    while(!(PLLSTAT & PLOCK)) ;
    
    // Connect the PLL as the clock source
    PLLCON=0x3;
    PllFeed();
    
    // Enabling MAM and setting number of clocks used for Flash memory fetch (4 cclks in this case)
    //MAMTIM=0x3; //VCOM?
    MAMCR=0x2;
    MAMTIM=0x4; //Original
    
    // Setting peripheral Clock (pclk) to System Clock (cclk)   72Mhz
    VPBDIV=0x1;

//    P0DIR |= (1<<STAT1)|(1<<STAT0);
    
//    PINSEL1 |= (2<<18);  // AOUT is on P0.25 (logomatic P4)
//    SCS= 0b11;
    PINSEL0 = 0;
    P0DIR &= ~(3);
    P0DIR |= ((1<<12)|(1<<30));
}
portEXIT_CRITICAL();
      
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
}

extern "C" void PllFeed( void )
{
    PLLFEED=0xAA;
    PLLFEED=0x55;
}

int main( void )
{
   
   

   bootup();
   P1DIR |=ph_a_high|ph_a_low|ph_b_high|ph_b_low|ph_c_high|ph_c_low; // output
   P1DIR &= ~(hall_a|hall_b|hall_c);
   initADC();

   initTimer1();
   
   P0CLR = 1<<30;  //power led
   buzzer(0);
   delay_ms(1000);
   uart_init();

   i2c_init();
    
   
   P0DIR |= power_on;// output
   P0SET = power_on;
   pwm_init();
   set_pwm(2);
  
  

   xTaskCreate(CountingTask,
               reinterpret_cast<const signed char *>("TCK"),
               TASK_STACK_SIZE,
               &CountingArgs,
               COUNTING_TASK_PRIORITY,
               &xCountingTaskHandle
               );

   
   xTaskCreate(PIDTask,
               reinterpret_cast<const signed char *>("PID"),
               TASK_STACK_SIZE,
               &PIDArgs,
               PID_TASK_PRIORITY,
               &xPIDTaskHandle
               );

  xTaskCreate(HallTask,
               reinterpret_cast<const signed char *>("Hal"),
               TASK_STACK_SIZE,
               &HallArgs,
               Hall_TASK_PRIORITY,
               &xHALLTaskHandle
               );

   vTaskStartScheduler();
   
   return 0;
} // main



void  pwm_init(void)
{
  
    PCONP |= 1<<5;
    PINSEL0 = ( PINSEL0 & (~(1<<16)))|(1<<17);
    PWMPR=1;   //pclk=72Mhz   pwm clock=72M/36=2MHZ
    PWMMCR = 1<<1;
    PWMMR0=100; // 2M/100=20KHZ PWM
    PWMMR4=0;
    PWMPCR |= 1<<12;  //ENABLE PWM OUTPUT4 AND SINGLE EDGE CONTROL
    PWMLER = (1<<0)|(1<<4);
    PWMTCR = 1|(1<<3);//ENABLE COUNTER AND PWM
}

void set_pwm(unsigned int _pwm_duty)
{
   if ((_pwm_duty<45)&&(_pwm_duty>30)&&(precharge_delay==0))
   { 
      precharge_delay=200;
   }
   if(precharge_delay==0)
   {
      PWMMR4=(_pwm_duty+1);
      PWMLER = 1<<4;  //ENABLE LATCH RESIGTER
   }
   else
   {
      pwm_duty=_pwm_duty+1;
      PWMMR4=50;
      PWMLER = 1<<4;  //ENABLE LATCH RESIGTER
   }
}




//     ---------+---+---+---+---+---+---+
//     Phase A  | - | - | Z | + | + | Z |
//     Phase B  | + | Z | - | - | Z | + |
//     Phase C  | Z | + | + | Z | - | - |
//     ---------+---+---+---+---+---+---+
//     Hall A   | 1 | 1 | 1 | 0 | 0 | 0 |
//     Hall B   | 0 | 0 | 1 | 1 | 1 | 0 |
//     Hall C   | 1 | 0 | 0 | 0 | 1 | 1 |
//     ---------+---+---+---+---+---+---+
void HallTask (void *pvParameters)
{
  int hall_val=0;
  int pre_hall_val=0;
  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
  P1CLR = ph_a_high|ph_a_low|ph_b_high|ph_b_low|ph_c_high|ph_c_low;
  
  char temp_alt=0;
//  P1DIR |= (1<<27)|(1<<28)|(1<<29);
  while(true)
  {

    int pinval = P1PIN;
    hall_val = ((pinval & hall_a) |((pinval&hall_b))|((pinval&hall_c)))>>24 ;// read hall a b c
    if (precharge_delay>1)
    {
      set_pwm(50);
      P1CLR=ph_a_high|ph_c_high|ph_b_high;
      P1SET=ph_a_low|ph_c_low|ph_b_low;
      precharge_delay--;
      P0CLR = 1<<30;  //power led
    }
    else
    {
      if (precharge_delay==1)
      {
        P1CLR = ph_a_high|ph_a_low|ph_b_high|ph_b_low|ph_c_high|ph_c_low; // TURN OFF MOTOR
        set_pwm(pwm_duty);
        precharge_delay--;
        P0SET = 1<<30;  //power led
      }
      
      if (pre_hall_val!=hall_val)   // state change
      {
        P1CLR = ph_a_high|ph_a_low|ph_b_high|ph_b_low|ph_c_high|ph_c_low; // TURN OFF MOTOR
      }
      else
      {
        switch (hall_val)
        {
          case (0b101):  
          {
            P1CLR = ph_a_high|ph_b_low|ph_c_high|ph_c_low;
            P1SET=ph_b_high|ph_a_low;
          }
          break;
          case (0b001):  
          {
            P1CLR = ph_a_high|ph_b_high|ph_b_low|ph_c_low;
            P1SET=ph_c_high|ph_a_low;
          }
          break;
          case (0b011):  
          {
            P1CLR = ph_a_high|ph_a_low|ph_b_high|ph_c_low;
            P1SET=ph_c_high|ph_b_low;
          }
          break;
          case (0b010):  
          {
            P1CLR = ph_a_low|ph_b_high|ph_c_high|ph_c_low;
            P1SET=ph_a_high|ph_b_low;
          }
          break;
          case (0b110):  
          {
            P1CLR = ph_a_low|ph_b_high|ph_b_low|ph_c_high;
            P1SET=ph_a_high|ph_c_low;
          }
          break;
          case (0b100):  
          {
            P1CLR = ph_a_high|ph_a_low|ph_b_low|ph_c_high;
            P1SET=ph_b_high|ph_c_low;
          }
          break;
          default:
          {
            P1CLR = ph_a_high|ph_a_low|ph_b_high|ph_b_low|ph_c_high|ph_c_low; // TURN OFF MOTOR
          }
        }
      }
    }
    pre_hall_val=hall_val;
    vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
  }
}